#!/usr/bin/python2

import thread
import threading
import Queue

matrix = [
[0.0, 0.82, 0.59, 0.68, 0.71, 0.31, 0.42, 0.05, 0.0, 0.0, 0.26, 0.0, 0.18, 0.56, 0.42, 0.45],
[0.59, 0.0, 0.72, 0.81, 0.16, 0.29, 0.0, 0.0, 0.0, 0.29, 0.0, 0.0, 0.87, 0.74, 0.08, 0.64],
[0.54, 0.0, 0.0, 0.08, 0.0, 0.43, 0.94, 1.0, 0.29, 0.95, 0.09, 0.71, 0.0, 0.51, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.81, 0.55, 0.0, 0.42, 0.43, 0.17, 0.0, 0.18, 0.97, 0.0, 0.97, 0.0],
[0.6, 0.55, 0.23, 0.81, 0.0, 0.12, 0.76, 0.0, 0.48, 0.0, 0.0, 0.99, 0.29, 0.27, 0.57, 0.0],
[0.0, 0.0, 0.72, 0.96, 0.0, 0.0, 0.0, 0.1, 0.93, 0.0, 0.78, 0.52, 0.95, 0.92, 0.48, 0.82],
[0.16, 0.74, 0.77, 0.0, 0.46, 0.23, 0.0, 0.0, 0.84, 0.85, 0.64, 0.54, 0.61, 0.59, 0.64, 0.32],
[0.76, 0.83, 0.59, 0.43, 0.0, 0.11, 0.66, 0.0, 0.29, 0.34, 0.18, 0.27, 0.78, 0.54, 0.22, 0.66],
[0.98, 0.84, 0.87, 0.43, 0.0, 0.0, 0.95, 0.38, 0.0, 0.1, 0.0, 0.38, 0.06, 0.0, 0.0, 0.0],
[0.14, 0.03, 0.0, 0.34, 0.51, 0.19, 0.55, 0.94, 0.72, 0.0, 0.0, 0.51, 0.0, 0.0, 0.25, 0.16],
[0.83, 0.91, 0.98, 0.45, 0.0, 0.68, 0.31, 0.0, 0.86, 0.4, 0.0, 0.52, 0.0, 0.91, 0.63, 0.31],
[0.88, 0.0, 0.26, 0.0, 0.57, 0.54, 0.09, 0.24, 0.0, 0.0, 0.0, 0.0, 0.0, 0.56, 0.96, 0.11],
[0.07, 0.85, 0.69, 0.76, 0.0, 0.52, 0.42, 0.74, 0.05, 0.88, 0.86, 0.15, 0.0, 0.0, 0.73, 0.87],
[0.48, 0.0, 0.0, 0.99, 0.61, 0.94, 0.37, 0.33, 0.96, 0.62, 0.03, 0.22, 0.8, 0.0, 0.97, 0.68],
[0.74, 0.02, 0.28, 0.2, 0.42, 0.0, 0.85, 0.37, 0.67, 0.89, 0.85, 0.65, 0.66, 0.14, 0.0, 0.0],
[0.73, 0.0, 0.28, 0.38, 0.65, 0.72, 0.98, 0.49, 0.07, 0.21, 0.0, 0.0, 0.46, 0.97, 0.53, 0.0]]

def dijkstra(matrix, s, t):
  assert s < 16  and s >= 0 and t < 16 and t >= 0, 'Invalid node range'
  S = {s: (1, None)}
  
  # Nodes are represented as a dictionary, where the key is the nodename
  # and the values are a tuple of (weight, predecessor)
  V = { node: (1, None) for node in range(0, len(matrix[0])) }
  
  while len(S) != len(V):
    # Candidates: All nodes that are not in S
    # We do not need to check that they are neighbors
    # of a vertex of S, because
    # the path is strongly connected.
    U = {node:value for node, value in V.items() if node not in S}
    
    distances = dict()
    for u, u_value in U.items():
      # All nodes in V that are in S are predecessors of u,
      # because the graph is strongly connected.
      for pre, pre_value in S.items():
        # weight(s, u) + weight(pre, u)
        distances[(u, pre)] = u_value[0] * matrix[pre][u]

    # find the largest probability
    max_dist = max(distances.values())
    # and get the nodes with the largest probability
    maxu, maxpre = [edge for edge in distances 
                if distances[edge] == max_dist][0]
    # update the accumulated weight of the node
    S[maxu] = (max_dist * S[maxpre][0] , maxpre)
    # When the maxnode is the target node, we're done
    if maxu == t:
      print(S[maxu])
      
def dijkstra_bidirectional(matrix, s, Queue):
  S = {s: (1, None)}
  
  # Nodes are represented as a dictionary, where the key is the nodename
  # and the values are a tuple of (weight, predecessor)
  V = { node: (1, None) for node in range(0, len(matrix[0])) }
  
  while len(S) != len(V):
    # Candidates: All nodes that are not in S
    # We do not need to check that they are neighbors
    # of a vertex of S, becausethe path is strongly connected.
    U = {node:value for node, value in V.items() if node not in S}
    
    distances = dict()
    for u, u_value in U.items():
      # All nodes in V that are in S are predecessors of u,
      # because the graph is strongly connected.
      for pre, pre_value in S.items():
        # weight(s, u) + weight(pre, u)
        distances[(u, pre)] = u_value[0] * matrix[pre][u]

    # find the largest probability
    max_dist = max(distances.values())
    # and get the nodes with the largest probability
    maxu, maxpre = [edge for edge in distances 
                  if distances[edge] == max_dist][0]
    # update the accumulated weight of the node
    weight = max_dist * S[maxpre][0]
    S[maxu] = (weight , maxpre)
    # add the max edge to the queue
    Queue.put( ((maxu, maxpre), weight) )
      
class Checker(threading.Thread):
  def __init__(self, q1, q2):
    threading.Thread.__init__(self) 
    self.q1 = q1
    self.q2 = q2
    self.forward_edges = dict()
    self.backward_edges = dict()
    self.current_best_path_distance = 1
    
  # We have found a path between s and t, namely s ... v w ... t.
  def check_match(self, edge, where):
    for e in where:
      if edge[1] == e[1]:
        return e
    return None
  
  def run(self):
    while True:
      forward_edge = self.q1.get()
      self.forward_edges[forward_edge[0]] = forward_edge[1]
      # print('forward edge ', forward_edge)
      match = self.check_match(forward_edge[0], self.backward_edges)
      if match:
        self.current_best_path_distance = self.backward_edges[match] * forward_edge[1]
        break
        
      backward_edge = self.q2.get()
      self.backward_edges[backward_edge[0]] = backward_edge[1]
      # print('backward edge ', backward_edge)
      match = self.check_match(backward_edge[0], self.forward_edges)
      if match:
        self.current_best_path_distance = self.forward_edges[match] * backward_edge[1]
        break
    print(self.current_best_path_distance)
           
def bidirectional_dijkstra(matrix, s, t):
  q1 = Queue.Queue()
  q2 = Queue.Queue()
  checker = Checker(q1, q2)
  checker.start()
  
  thread.start_new_thread(dijkstra_bidirectional, (matrix, s, q1))
  thread.start_new_thread(dijkstra_bidirectional, (matrix, t, q2))
  
def simple_dijkstra_example():
  """We may find the shortest path from s to t with native djikstra"""
  # Stairway to Heaven @ 53.59975 / 9.93170
  # (0.6206579999999999, 12)
  # (0.87, 1)
  # (0.7694680199999999, 13)
  for f, t in [
        (0, 15),
        (1, 12),
        (3, 5),
      ]:
    dijkstra(matrix, f, t)
    
def bidirectional_dijkstra_example():
  """We may also find the shortest path from s to t
  with bidirectional djikstra which is more efficient.
  """
  bidirectional_dijkstra(matrix, 0, 15)
  bidirectional_dijkstra(matrix, 1, 12)
  bidirectional_dijkstra(matrix, 3, 5)
    
if __name__ == '__main__':
  simple_dijkstra_example()
  # bidirectional_dijkstra_example()
